/*
 * Gyroscope.cpp
 *
 *  Created on: 22.11.2012
 *      Author: Michael
 */

#include "rodos.h"
#include "Gyroscope.h"

Gyroscope::Gyroscope(I2C *i2c, int range) {
	this->i2c = i2c;

	// TODO: configure function
	if (read(WHO_AM_I) == L3G4200D_ADDR + 1) {		// Check if gyroscope is available
		write(CTRL_REG1, 0x0C);						// enable normal mode on z axis

		switch (range) {							// set measurement range
		case dps250:
			gyroScale = dps250Scale;
			write(CTRL_REG4, 0x00);
			// TODO: Raw Bias 250 dps
			rawBias = 0;
			break;
		case dps500:
			gyroScale = dps500Scale;
			write(CTRL_REG4, 0x10);
			rawBias = dps500Bias;
			break;
		case dps2000:
			gyroScale = dps2000Scale;
			write(CTRL_REG4, 0x20);
			rawBias = dps2000Bias;
			break;
		default:
			break;
		}
	} else {
//		xprintf("Gyroscope INIT ERROR");
	}
}

Gyroscope::~Gyroscope() {

}

void Gyroscope::write(uint8_t regAddr, uint8_t val) {
	i2c->write(L3G4200D_ADDR, regAddr, val);
}

uint8_t Gyroscope::read(uint8_t regAddr) {
	return i2c->read(L3G4200D_ADDR, regAddr);
}

int Gyroscope::getRawAngularRateZ() {
	uint8_t high_z = read(OUT_Z_H);
	uint8_t low_z = read(OUT_Z_L);
	int16_t rot_z = (int16_t) ((high_z << 8) | low_z);
	return rot_z;
}

int Gyroscope::getRawTemp() {
	uint8_t temp = read(OUT_TEMP);
	return temp;
}

float Gyroscope::getAngularRateZ(){
	float raw_z = getRawAngularRateZ();
	float rot_z = ((float) raw_z - rawBias)*gyroScale;
	return rot_z;
}

int Gyroscope::getTemp(){
	int temp = REFTEMP - (int) getRawTemp();
	return temp;
}

float Gyroscope::getScaledAngularRateZ(){
	float raw_z = getRawAngularRateZ();
	return ((float) raw_z) * gyroScale;
}


